#define TIRERECOGNITIONDLL_LIBRARY
#include "TireRecognition.h"
#include <stdio.h>
#include <time.h>
#include <math.h>

TireRecognition* createClass_TireRecognition() {
    return new TireRecognition();
}

TireRecognition::TireRecognition() {
    usedFunction = 0;
    MagicFormula::inputFzoRo(0, 0);
}

TireRecognition::~TireRecognition() {}

int TireRecognition::fileOpen(const char* fname) { //checked
    for (int i = 0; i<7; ++i) {
        data[i].clear();
    }
    double data_in[7];

    FILE* file;
    fopen_s(&file, fname, "r");
    if (NULL == file) {
        return -1;
    }
    double temp;
    while (!feof(file)) {
        //魔术公式有5个变量，从序号0到6依次为alpha、r、k、Fx、Fy、Fz、Mz
        if (EOF == fscanf(file, "%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\n",
                          &temp, &data_in[0], &data_in[1], &data_in[2], &data_in[3], &data_in[4],
                          &data_in[5], &temp, &data_in[6], &temp, &temp)) {
            return -1;
        }
        for (int i = 0; i<7; ++i) {
            data[i].push_back(data_in[i]);
        }
    }
    fclose(file);
    return 0;
}

int TireRecognition::dataNumber(int colms) {
    if (colms < 7 && colms >= 0) {
        return data[colms].size();
    } else {
        return -1;
    }
}

int TireRecognition::computePrivateFx(const int iterNumber, const double eOut, double lamda, const double v) {
    clock_t timep = clock();

    const int parametersNumber = 15;    //函数参数的个数。PrivateFx方程有15个系数
    double parametersEst[parametersNumber];		//迭代中进行计算的参数
    double parametersLm[parametersNumber];		//迭代中进行计算的参数,基本上LM是EST的下一个

    //对估计参数赋初值，parametersEst为最后求得的参数数组
    InitialPrivateFx(parametersEst);

    int dataNumber = data[0].size();	//数据个数

    //计算雅可比矩阵和残差，将雅可比矩阵改为一个数组内包含所有变量
    std::vector<double*> dataJacobian;	//方程的雅可比矩阵
    std::vector<double> residual;		//每次实际data[0]和理论data[0]的误差
    double temp;	//临时浮点数
    double e = 0, eLm;		//残差之和，Lm是新的
    bool iterOn = 1;	//是否进行迭代
    double hessian[parametersNumber*parametersNumber];	//海塞矩阵，用一位数组表示
    double hessianLm[parametersNumber*parametersNumber];
    double gTemp[parametersNumber] = { 0 };		//用于计算步长step[]

    //分配内存，加快速度。总是分配内存影响性能
    for (int i = 0; i<dataNumber; ++i) {
        double* parametersJacobian = new double[parametersNumber];	//新建参数数组，记得删除
        double residual_temp;
        dataJacobian.push_back(parametersJacobian);
        residual.push_back(residual_temp);
    }

    //计算循环开始
    for (int m = 0; m<iterNumber; ++m) {
        if (iterOn) {
            //计算雅可比矩阵 和残差   checked
            for (int i = 0; i<dataNumber; ++i) {
                //计算雅可比矩阵，矩阵为parametersNumber行，dataNumber列
                //				double alpha=data[0].at(i);
                double r = data[1].at(i);
                double k = data[2].at(i);
                //				double Fx=data[3].at(i);
                //				double Fy=data[4].at(i);
                double Fz = data[5].at(i);

                //计算每个点的残差
                residual.at(i) = data[3].at(i) - PrivateFx(Fz, k, r, parametersEst);

                double* parametersJacobian = dataJacobian.at(i);
                parametersJacobian[0] = PrivateFx_00(Fz, k, r, parametersEst);
                parametersJacobian[1] = PrivateFx_01(Fz, k, r, parametersEst);
                parametersJacobian[2] = PrivateFx_02(Fz, k, r, parametersEst);
                parametersJacobian[3] = PrivateFx_03(Fz, k, r, parametersEst);
                parametersJacobian[4] = PrivateFx_04(Fz, k, r, parametersEst);
                parametersJacobian[5] = PrivateFx_05(Fz, k, r, parametersEst);
                parametersJacobian[6] = PrivateFx_06(Fz, k, r, parametersEst);
                parametersJacobian[7] = PrivateFx_07(Fz, k, r, parametersEst);
                parametersJacobian[8] = PrivateFx_08(Fz, k, r, parametersEst);
                parametersJacobian[9] = PrivateFx_09(Fz, k, r, parametersEst);
                parametersJacobian[10] = PrivateFx_10(Fz, k, r, parametersEst);
                parametersJacobian[11] = PrivateFx_11(Fz, k, r, parametersEst);
                parametersJacobian[12] = PrivateFx_12(Fz, k, r, parametersEst);
                parametersJacobian[13] = PrivateFx_13(Fz, k, r, parametersEst);
                parametersJacobian[14] = PrivateFx_14(Fz, k, r, parametersEst);
            }
            //如果是第一次计算就计算残差和
            if (m == 0) {
                for (int i = 0; i<dataNumber; ++i) {
                    e += residual.at(i)*residual.at(i);
                }
                //printf("%f, m=%d\n", e, m);
            }
            //计算海塞矩阵 checked
            for (int i = 0; i<parametersNumber; ++i) {
                for (int j = 0; j<parametersNumber; ++j) {
                    temp = 0;
                    for (int k = 0; k<dataNumber; ++k) {
                        temp += (dataJacobian.at(k))[i] * (dataJacobian.at(k))[j];
                    }
                    hessian[parametersNumber*i + j] = temp;
                }
            }
            //计算用于计算步长step[]的gTemp[]
            for (int i = 0; i<parametersNumber; ++i) {
                gTemp[i] = 0;
                for (int j = 0; j<dataNumber; ++j) {
                    gTemp[i] += (dataJacobian.at(j))[i] * residual.at(j);
                }
            }
        }

        //计算矩阵hessianLm=hessian+(lamda*identity),identity为parametersNumber阶的单位矩阵
        for (int i = 0; i<parametersNumber; ++i) {
            for (int j = 0; j<parametersNumber; ++j) {
                hessianLm[parametersNumber*i + j] = hessian[parametersNumber*i + j];
            }
            hessianLm[parametersNumber*i + i] += lamda;
        }
        //矩阵hessianLm求逆
        if (inverse(hessianLm, parametersNumber)) {
            return -1;
        }

        //求step=inv(hessian)*(dataJacobian*residual)  checked
        double step[parametersNumber] = { 0 };
        for (int i = 0; i<parametersNumber; ++i) {
            for (int j = 0; j<parametersNumber; ++j) {
                step[i] += hessianLm[parametersNumber*i + j] * gTemp[j];
            }
        }
        //更新parametersLm
        for (int i = 0; i<parametersNumber; ++i) {
            parametersLm[i] = parametersEst[i] + step[i];
        }

        //计算新的可能估计值对应的y和计算残差e  checked
        eLm = 0;
        for (int i = 0; i<dataNumber; ++i) {
            temp = data[3].at(i) - PrivateFx(data[5].at(i), data[2].at(i), data[1].at(i), parametersLm);
            eLm += temp*temp;
        }

        //达到eOut的精度退出
        if (eOut >= ((eLm>e) ? (eLm - e) : (e - eLm))) {
            if (eLm<e) {
                for (int i = 0; i<parametersNumber; ++i) {
                    parametersEst[i] = parametersLm[i];
                }
            }
            break;
        }
        //根据残差，更新参数
        if (eLm<e) {
            //printf("%f, m=%d\n", eLm, m);
            lamda /= v;
            e = eLm;
            for (int i = 0; i<parametersNumber; ++i) {
                parametersEst[i] = parametersLm[i];
            }
            iterOn = 1;
        } else {
            lamda *= v;
            iterOn = 0;
        }
    }

    //退出
    for (int i = 0; i<parametersNumber; ++i) {		//将结果复制到目标数组
        parametersPrivateFx[i] = parametersEst[i];
    }
    if (!dataJacobian.empty()) {
        for (int i = 0; i<dataNumber; ++i) {
            delete[](dataJacobian.at(i));
        }
    }
    timep = clock() - timep;
    //printf("%f, time=%ld\n", e - eLm, timep);
    return timep;
}

int TireRecognition::computePrivateFy(const int iterNumber, const double eOut, double lamda, const double v) {
    clock_t timep = clock();

    const int parametersNumber = 18;	//函数参数的个数。PrivateFx方程有15个系数
    double parametersEst[parametersNumber];		//迭代中进行计算的参数
    double parametersLm[parametersNumber];		//迭代中进行计算的参数,基本上LM是EST的下一个

    //对估计参数赋初值，parametersEst为最后求得的参数数组
    InitialPrivateFy(parametersEst);

    int dataNumber = data[0].size();	//数据个数

    //计算雅可比矩阵和残差，将雅可比矩阵改为一个数组内包含所有变量
    std::vector<double*> dataJacobian;	//方程的雅可比矩阵
    std::vector<double> residual;		//每次实际data[0]和理论data[0]的误差
    double temp;	//临时浮点数
    double e = 0, eLm;		//残差之和，Lm是新的
    bool iterOn = 1;	//是否进行迭代
    double hessian[parametersNumber*parametersNumber];	//海塞矩阵，用一位数组表示
    double hessianLm[parametersNumber*parametersNumber];
    double gTemp[parametersNumber] = { 0 };		//用于计算步长step[]

    //分配内存，加快速度。总是分配内存影响性能
    for (int i = 0; i<dataNumber; ++i) {
        double* parametersJacobian = new double[parametersNumber];	//新建参数数组，记得删除
        double residual_temp;
        dataJacobian.push_back(parametersJacobian);
        residual.push_back(residual_temp);
    }

    //计算循环开始
    for (int m = 0; m<iterNumber; ++m) {
        if (iterOn) {
            //计算雅可比矩阵 和残差   checked
            for (int i = 0; i<dataNumber; ++i) {
                //------------------------修改这里--------------------------------------------------------
                double alpha = data[0].at(i);
                double r = data[1].at(i);
                //				double k=data[2].at(i);
                //				double Fx=data[3].at(i);
                //				double Fy=data[4].at(i);
                double Fz = data[5].at(i);

                //计算每个点的残差
                temp=PrivateFy(Fz, alpha, r, parametersEst);
                residual.at(i) = data[4].at(i) - PrivateFy(Fz, alpha, r, parametersEst);

                //计算雅可比矩阵，矩阵为parametersNumber行，dataNumber列
                double* parametersJacobian = dataJacobian.at(i);
                parametersJacobian[0] = PrivateFy_00(parametersEst);
                parametersJacobian[1] = PrivateFy_01(Fz, r, parametersEst);
                parametersJacobian[2] = PrivateFy_02(Fz, r, parametersEst);
                parametersJacobian[3] = PrivateFy_03(Fz, r, parametersEst);
                parametersJacobian[4] = PrivateFy_04(r, parametersEst);
                parametersJacobian[5] = PrivateFy_05(r, parametersEst);
                parametersJacobian[6] = PrivateFy_06(parametersEst);
                parametersJacobian[7] = PrivateFy_07(r, parametersEst);
                parametersJacobian[8] = PrivateFy_08(Fz, r, parametersEst);
                parametersJacobian[9] = PrivateFy_09(Fz, r, parametersEst);
                parametersJacobian[10] = PrivateFy_10(Fz, r, parametersEst);
                parametersJacobian[11] = PrivateFy_11(parametersEst);
                parametersJacobian[12] = PrivateFy_12(parametersEst);
                parametersJacobian[13] = PrivateFy_13(r, parametersEst);
                parametersJacobian[14] = PrivateFy_14(Fz);
                parametersJacobian[15] = PrivateFy_15(Fz);
                parametersJacobian[16] = PrivateFy_16(Fz, r);
                parametersJacobian[17] = PrivateFy_17(Fz, r);
                //-----------------------------------------------------------------------------------------
            }
            //如果是第一次计算就计算残差和
            if (m == 0) {
                for (int i = 0; i<dataNumber; ++i) {
                    e += residual.at(i)*residual.at(i);
                }
                //printf("%f, m=%d\n", e, m);
            }
            //计算海塞矩阵 checked
            for (int i = 0; i<parametersNumber; ++i) {
                for (int j = 0; j<parametersNumber; ++j) {
                    temp = 0;
                    for (int k = 0; k<dataNumber; ++k) {
                        temp += (dataJacobian.at(k))[i] * (dataJacobian.at(k))[j];
                    }
                    hessian[parametersNumber*i + j] = temp;
                }
            }
            //计算用于计算步长step[]的gTemp[]
            for (int i = 0; i<parametersNumber; ++i) {
                gTemp[i] = 0;
                for (int j = 0; j<dataNumber; ++j) {
                    gTemp[i] += (dataJacobian.at(j))[i] * residual.at(j);
                }
            }
        }

        //计算矩阵hessianLm=hessian+(lamda*identity),identity为parametersNumber阶的单位矩阵
        for (int i = 0; i<parametersNumber; ++i) {
            for (int j = 0; j<parametersNumber; ++j) {
                hessianLm[parametersNumber*i + j] = hessian[parametersNumber*i + j];
            }
            hessianLm[parametersNumber*i + i] += lamda;
        }
        //矩阵hessianLm求逆
        if (inverse(hessianLm, parametersNumber)) {
            return -1;
        }

        //求step=inv(hessian)*(dataJacobian*residual)  checked
        double step[parametersNumber] = { 0 };
        for (int i = 0; i<parametersNumber; ++i) {
            for (int j = 0; j<parametersNumber; ++j) {
                step[i] += hessianLm[parametersNumber*i + j] * gTemp[j];
            }
        }
        //更新parametersLm
        for (int i = 0; i<parametersNumber; ++i) {
            parametersLm[i] = parametersEst[i] + step[i];
        }

        //计算新的可能估计值对应的y和计算残差e  checked
        eLm = 0;
        for (int i = 0; i<dataNumber; ++i) {
            //------------------------修改这里--------------------------------------------------
            temp = data[4].at(i) - PrivateFy(data[5].at(i), data[0].at(i), data[1].at(i), parametersLm);
            eLm += temp*temp;
            //---------------------------------------------------------------------------------
        }

        //达到eOut的精度退出
        if (eOut >= ((eLm>e) ? (eLm - e) : (e - eLm))) {
            if (eLm<e) {
                for (int i = 0; i<parametersNumber; ++i) {
                    parametersEst[i] = parametersLm[i];
                }
            }
            break;
        }
        //根据残差，更新参数
        if (eLm<e) {
            //printf("%f, m=%d\n", eLm, m);
            lamda /= v;
            e = eLm;
            for (int i = 0; i<parametersNumber; ++i) {
                parametersEst[i] = parametersLm[i];
            }
            iterOn = 1;
        } else {
            lamda *= v;
            iterOn = 0;
        }
    }

    //退出
    for (int i = 0; i<parametersNumber; ++i) {		//将结果复制到目标数组
        parametersPrivateFy[i] = parametersEst[i];
    }
    if (!dataJacobian.empty()) {
        for (int i = 0; i<dataNumber; ++i) {
            delete[](dataJacobian.at(i));
        }
    }
    timep = clock() - timep;
    //printf("%f, time=%ldms\n", e - eLm, timep);
    return timep;
}

int TireRecognition::computePrivateMz(const int iterNumber, const double eOut, double lamda, const double v) {
    clock_t timep = clock();

    const int parametersNumber = 25;	//函数参数的个数。PrivateMz方程有25个系数
    double parametersEst[parametersNumber];		//迭代中进行计算的参数
    double parametersLm[parametersNumber];		//迭代中进行计算的参数,基本上LM是EST的下一个

    //对估计参数赋初值，parametersEst为最后求得的参数数组
    InitialPrivateMz(parametersEst);

    int dataNumber = data[0].size();	//数据个数

    //计算雅可比矩阵和残差，将雅可比矩阵改为一个数组内包含所有变量
    std::vector<double*> dataJacobian;	//方程的雅可比矩阵
    std::vector<double> residual;		//每次实际data[0]和理论data[0]的误差
    double temp;	//临时浮点数
    double e = 0, eLm;		//残差之和，Lm是新的
    bool iterOn = 1;	//是否进行迭代
    double hessian[parametersNumber*parametersNumber];	//海塞矩阵，用一位数组表示
    double hessianLm[parametersNumber*parametersNumber];
    double gTemp[parametersNumber] = { 0 };		//用于计算步长step[]

    //分配内存，加快速度。总是分配内存影响性能
    for (int i = 0; i<dataNumber; ++i) {
        double* parametersJacobian = new double[parametersNumber];	//新建参数数组，记得删除
        double residual_temp;
        dataJacobian.push_back(parametersJacobian);
        residual.push_back(residual_temp);
    }

    //计算循环开始
    for (int m = 0; m<iterNumber; ++m) {
        if (iterOn) {
            //计算雅可比矩阵 和残差   checked
            for (int i = 0; i<dataNumber; ++i) {
                //------------------------修改这里--------------------------------------------------------
                double alpha = data[0].at(i);
                double r = data[1].at(i);
                //				double k=data[2].at(i);
                //				double Fx=data[3].at(i);
                //				double Fy=data[4].at(i);
                double Fz = data[5].at(i);
                //				double Mz=data[6].at(i);

                //计算每个点的残差

                residual.at(i) = data[6].at(i) - PrivateMz(Fz, alpha, r, parametersEst);
                //计算雅可比矩阵，矩阵为parametersNumber行，dataNumber列
                double* parametersJacobian = dataJacobian.at(i);
                parametersJacobian[0] = PrivateMz_00(alpha, r, parametersEst);
                parametersJacobian[1] = PrivateMz_01(alpha, r, parametersEst);
                parametersJacobian[2] = PrivateMz_02(alpha, r, parametersEst);
                parametersJacobian[3] = PrivateMz_03(alpha, r, parametersEst);
                parametersJacobian[4] = PrivateMz_04(alpha, r, parametersEst);
                parametersJacobian[5] = PrivateMz_05(alpha);
                parametersJacobian[6] = PrivateMz_06(alpha);
                parametersJacobian[7] = PrivateMz_07(alpha, r, parametersEst);
                parametersJacobian[8] = PrivateMz_08(Fz, alpha, r, parametersEst);
                parametersJacobian[9] = PrivateMz_09(Fz, alpha, r, parametersEst);
                parametersJacobian[10] = PrivateMz_10(Fz, alpha, r, parametersEst);
                parametersJacobian[11] = PrivateMz_11(Fz, alpha, r, parametersEst);
                parametersJacobian[12] = PrivateMz_12(Fz, alpha);
                parametersJacobian[13] = PrivateMz_13(Fz, alpha);
                parametersJacobian[14] = PrivateMz_14(Fz, alpha, r);
                parametersJacobian[15] = PrivateMz_15(Fz, alpha, r);
                parametersJacobian[16] = PrivateMz_16(alpha, r, parametersEst);
                parametersJacobian[17] = PrivateMz_17(alpha, r, parametersEst);
                parametersJacobian[18] = PrivateMz_18(alpha, r, parametersEst);
                parametersJacobian[19] = PrivateMz_19(alpha, parametersEst);
                parametersJacobian[20] = PrivateMz_20(alpha, r, parametersEst);
                parametersJacobian[21] = PrivateMz_21(alpha, parametersEst);
                parametersJacobian[22] = PrivateMz_22(alpha, parametersEst);
                parametersJacobian[23] = PrivateMz_23(alpha, r, parametersEst);
                parametersJacobian[24] = PrivateMz_24(alpha, r, parametersEst);
                //-------------------------------------------------------------------------------------------
            }
            //如果是第一次计算就计算残差和
            if (m == 0) {
                for (int i = 0; i<dataNumber; ++i) {
                    e += residual.at(i)*residual.at(i);
                }
                //printf("%f, m=%d\n", e, m);
            }
            //计算海塞矩阵 checked
            for (int i = 0; i<parametersNumber; ++i) {
                for (int j = 0; j<parametersNumber; ++j) {
                    temp = 0;
                    for (int k = 0; k<dataNumber; ++k) {
                        temp += (dataJacobian.at(k))[i] * (dataJacobian.at(k))[j];
                    }
                    hessian[parametersNumber*i + j] = temp;
                }
            }
            //计算用于计算步长step[]的gTemp[]
            for (int i = 0; i<parametersNumber; ++i) {
                gTemp[i] = 0;
                for (int j = 0; j<dataNumber; ++j) {
                    gTemp[i] += (dataJacobian.at(j))[i] * residual.at(j);
                }
            }
        }

        //计算矩阵hessianLm=hessian+(lamda*identity),identity为parametersNumber阶的单位矩阵
        for (int i = 0; i<parametersNumber; ++i) {
            for (int j = 0; j<parametersNumber; ++j) {
                hessianLm[parametersNumber*i + j] = hessian[parametersNumber*i + j];
            }
            hessianLm[parametersNumber*i + i] += lamda;
        }
        //矩阵hessianLm求逆
        if (inverse(hessianLm, parametersNumber)) {
            return -1;
        }

        //求step=inv(hessian)*(dataJacobian*residual)  checked
        double step[parametersNumber] = { 0 };
        for (int i = 0; i<parametersNumber; ++i) {
            for (int j = 0; j<parametersNumber; ++j) {
                step[i] += hessianLm[parametersNumber*i + j] * gTemp[j];
            }
        }
        //更新parametersLm
        for (int i = 0; i<parametersNumber; ++i) {
            parametersLm[i] = parametersEst[i] + step[i];
        }

        //计算新的可能估计值对应的y和计算残差e  checked
        eLm = 0;
        for (int i = 0; i<dataNumber; ++i) {
            //------------------------修改这里--------------------------------------------------
            temp = data[6].at(i) - PrivateMz(data[5].at(i), data[0].at(i), data[1].at(i), parametersLm);
            eLm += temp*temp;
            //---------------------------------------------------------------------------------
        }

        //达到eOut的精度退出
        if (eOut >= ((eLm>e) ? (eLm - e) : (e - eLm))) {
            if (eLm<e) {
                for (int i = 0; i<parametersNumber; ++i) {
                    parametersEst[i] = parametersLm[i];
                }
            }
            break;
        }
        //根据残差，更新参数
        if (eLm<e) {
            //printf("%f, m=%d\n", eLm, m);
            lamda /= v;
            e = eLm;
            for (int i = 0; i<parametersNumber; ++i) {
                parametersEst[i] = parametersLm[i];
            }
            iterOn = 1;
        } else {
            lamda *= v;
            iterOn = 0;
        }
    }

    //退出
    for (int i = 0; i<parametersNumber; ++i) {		//将结果复制到目标数组
        parametersPrivateMz[i] = parametersEst[i];
    }
    if (!dataJacobian.empty()) {
        for (int i = 0; i<dataNumber; ++i) {
            delete[](dataJacobian.at(i));
        }
    }
    timep = clock() - timep;
    //printf("%f, time=%ldms\n", e - eLm, timep);
    return timep;
}


int TireRecognition::computePrivateCombFx(const int iterNumber, const double eOut, double lamda, const double v) {
    clock_t timep = clock();

    const int parametersNumber = 6;	//函数参数的个数。PrivateCombFx方程有6个系数
    double parametersEst[parametersNumber];		//迭代中进行计算的参数
    double parametersLm[parametersNumber];		//迭代中进行计算的参数,基本上LM是EST的下一个

    //对估计参数赋初值，parametersEst为最后求得的参数数组
    InitialPrivateCombFx(parametersEst);

    int dataNumber = data[0].size();	//数据个数

    //计算雅可比矩阵和残差，将雅可比矩阵改为一个数组内包含所有变量
    std::vector<double*> dataJacobian;	//方程的雅可比矩阵
    std::vector<double> residual;		//每次实际data[0]和理论data[0]的误差
    double temp;	//临时浮点数
    double e = 0, eLm;		//残差之和，Lm是新的
    bool iterOn = 1;	//是否进行迭代
    double hessian[parametersNumber*parametersNumber];	//海塞矩阵，用一位数组表示
    double hessianLm[parametersNumber*parametersNumber];
    double gTemp[parametersNumber] = { 0 };		//用于计算步长step[]

    //分配内存，加快速度。总是分配内存影响性能
    for (int i = 0; i<dataNumber; ++i) {
        double* parametersJacobian = new double[parametersNumber];	//新建参数数组，记得删除
        double residual_temp;
        dataJacobian.push_back(parametersJacobian);
        residual.push_back(residual_temp);
    }

    //计算循环开始
    for (int m = 0; m<iterNumber; ++m) {
        if (iterOn) {
            //计算雅可比矩阵 和残差   checked
            for (int i = 0; i<dataNumber; ++i) {
                //------------------------修改这里--------------------------------------------------------
                double alpha = data[0].at(i);
                double r = data[1].at(i);
                double k = data[2].at(i);
                //double Fx=data[3].at(i);
                //double Fy=data[4].at(i);
                double Fz = data[5].at(i);

                //计算每个点的残差
                residual.at(i) = data[3].at(i) - PrivateCombFx(Fz, k, alpha, r, parametersEst);

                //计算雅可比矩阵，矩阵为parametersNumber行，dataNumber列
                double* parametersJacobian = dataJacobian.at(i);
                parametersJacobian[0] = PrivateCombFx_00(k, parametersEst);
                parametersJacobian[1] = PrivateCombFx_01(k, parametersEst);
                parametersJacobian[2] = PrivateCombFx_02(parametersEst);
                parametersJacobian[3] = PrivateCombFx_03(parametersEst);
                parametersJacobian[4] = PrivateCombFx_04(parametersEst);
                parametersJacobian[5] = PrivateCombFx_05(parametersEst);
                //-------------------------------------------------------------------------------------------
            }
            //如果是第一次计算就计算残差和
            if (m == 0) {
                for (int i = 0; i<dataNumber; ++i) {
                    e += residual.at(i)*residual.at(i);
                }
                //printf("%f, m=%d\n", e, m);
            }
            //计算海塞矩阵 checked
            for (int i = 0; i<parametersNumber; ++i) {
                for (int j = 0; j<parametersNumber; ++j) {
                    temp = 0;
                    for (int k = 0; k<dataNumber; ++k) {
                        temp += (dataJacobian.at(k))[i] * (dataJacobian.at(k))[j];
                    }
                    hessian[parametersNumber*i + j] = temp;
                }
            }
            //计算用于计算步长step[]的gTemp[]
            for (int i = 0; i<parametersNumber; ++i) {
                gTemp[i] = 0;
                for (int j = 0; j<dataNumber; ++j) {
                    gTemp[i] += (dataJacobian.at(j))[i] * residual.at(j);
                }
            }
        }

        //计算矩阵hessianLm=hessian+(lamda*identity),identity为parametersNumber阶的单位矩阵
        for (int i = 0; i<parametersNumber; ++i) {
            for (int j = 0; j<parametersNumber; ++j) {
                hessianLm[parametersNumber*i + j] = hessian[parametersNumber*i + j];
            }
            hessianLm[parametersNumber*i + i] += lamda;
        }
        //矩阵hessianLm求逆
        if (inverse(hessianLm, parametersNumber)) {
            return -1;
        }

        //求step=inv(hessian)*(dataJacobian*residual)  checked
        double step[parametersNumber] = { 0 };
        for (int i = 0; i<parametersNumber; ++i) {
            for (int j = 0; j<parametersNumber; ++j) {
                step[i] += hessianLm[parametersNumber*i + j] * gTemp[j];
            }
        }
        //更新parametersLm
        for (int i = 0; i<parametersNumber; ++i) {
            parametersLm[i] = parametersEst[i] + step[i];
        }

        //计算新的可能估计值对应的y和计算残差e  checked
        eLm = 0;
        for (int i = 0; i<dataNumber; ++i) {
            //------------------------修改这里--------------------------------------------------
            temp = data[3].at(i) - PrivateCombFx(data[5].at(i), data[2].at(i), data[0].at(i),
                                                 data[1].at(i), parametersLm);
            eLm += temp*temp;
            //---------------------------------------------------------------------------------
        }

        //达到eOut的精度退出
        if (eOut >= ((eLm>e) ? (eLm - e) : (e - eLm))) {
            if (eLm<e) {
                for (int i = 0; i<parametersNumber; ++i) {
                    parametersEst[i] = parametersLm[i];
                }
            }
            break;
        }
        //根据残差，更新参数
        if (eLm<e) {
            //printf("%f, m=%d\n", eLm, m);
            lamda /= v;
            e = eLm;
            for (int i = 0; i<parametersNumber; ++i) {
                parametersEst[i] = parametersLm[i];
            }
            iterOn = 1;
        } else {
            lamda *= v;
            iterOn = 0;
        }
    }

    //退出
    for (int i = 0; i<parametersNumber; ++i) {		//将结果复制到目标数组
        parametersPrivateCombFx[i] = parametersEst[i];
    }
    if (!dataJacobian.empty()) {
        for (int i = 0; i<dataNumber; ++i) {
            delete[](dataJacobian.at(i));
        }
    }
    timep = clock() - timep;
    //printf("%f, time=%ldms\n", e - eLm, timep);
    return timep;
}

int TireRecognition::computePrivateCombFy(const int iterNumber, const double eOut, double lamda, const double v) {
    clock_t timep = clock();

    const int parametersNumber = 14;	//函数参数的个数。PrivateCombFy方程有14个系数
    double parametersEst[parametersNumber];		//迭代中进行计算的参数
    double parametersLm[parametersNumber];		//迭代中进行计算的参数,基本上LM是EST的下一个

    //对估计参数赋初值，parametersEst为最后求得的参数数组
    InitialPrivateCombFy(parametersEst);

    int dataNumber = data[0].size();	//数据个数

    //计算雅可比矩阵和残差，将雅可比矩阵改为一个数组内包含所有变量
    std::vector<double*> dataJacobian;	//方程的雅可比矩阵
    std::vector<double> residual;		//每次实际data[0]和理论data[0]的误差
    double temp;	//临时浮点数
    double e = 0, eLm;		//残差之和，Lm是新的
    bool iterOn = 1;	//是否进行迭代
    double hessian[parametersNumber*parametersNumber];	//海塞矩阵，用一位数组表示
    double hessianLm[parametersNumber*parametersNumber];
    double gTemp[parametersNumber] = { 0 };		//用于计算步长step[]

    //分配内存，加快速度。总是分配内存影响性能
    for (int i = 0; i<dataNumber; ++i) {
        double* parametersJacobian = new double[parametersNumber];	//新建参数数组，记得删除
        double residual_temp;
        dataJacobian.push_back(parametersJacobian);
        residual.push_back(residual_temp);
    }

    //计算循环开始
    for (int m = 0; m<iterNumber; ++m) {
        if (iterOn) {
            //计算雅可比矩阵 和残差   checked
            for (int i = 0; i<dataNumber; ++i) {
                //------------------------修改这里--------------------------------------------------------
                double alpha = data[0].at(i);
                double r = data[1].at(i);
                double k = data[2].at(i);
                //double Fx=data[3].at(i);
                //double Fy=data[4].at(i);
                double Fz = data[5].at(i);

                //计算每个点的残差
                residual.at(i) = data[4].at(i) - PrivateCombFy(Fz, k, alpha, r, parametersEst);

                //计算雅可比矩阵，矩阵为parametersNumber行，dataNumber列
                double* parametersJacobian = dataJacobian.at(i);
                parametersJacobian[0] = PrivateCombFy_00(alpha, parametersEst);
                parametersJacobian[1] = PrivateCombFy_01(alpha, parametersEst);
                parametersJacobian[2] = PrivateCombFy_02(alpha, parametersEst);
                parametersJacobian[3] = PrivateCombFy_03(parametersEst);
                parametersJacobian[4] = PrivateCombFy_04(parametersEst);
                parametersJacobian[5] = PrivateCombFy_05(parametersEst);
                parametersJacobian[6] = PrivateCombFy_06(parametersEst);
                parametersJacobian[7] = PrivateCombFy_07(parametersEst);
                parametersJacobian[8] = PrivateCombFy_08(Fz, k, alpha, parametersEst);
                parametersJacobian[9] = PrivateCombFy_09(Fz, k, alpha, parametersEst);
                parametersJacobian[10] = PrivateCombFy_10(Fz, k, alpha, r, parametersEst);
                parametersJacobian[11] = PrivateCombFy_11(Fz, k, alpha, r, parametersEst);
                parametersJacobian[12] = PrivateCombFy_12(Fz, k, alpha, r, parametersEst);
                parametersJacobian[13] = PrivateCombFy_13(Fz, k, alpha, r, parametersEst);
                //-------------------------------------------------------------------------------------------
            }
            //如果是第一次计算就计算残差和
            if (m == 0) {
                for (int i = 0; i<dataNumber; ++i) {
                    e += residual.at(i)*residual.at(i);
                }
                //printf("%f, m=%d\n", e, m);
            }
            //计算海塞矩阵 checked
            for (int i = 0; i<parametersNumber; ++i) {
                for (int j = 0; j<parametersNumber; ++j) {
                    temp = 0;
                    for (int k = 0; k<dataNumber; ++k) {
                        temp += (dataJacobian.at(k))[i] * (dataJacobian.at(k))[j];
                    }
                    hessian[parametersNumber*i + j] = temp;
                }
            }
            //计算用于计算步长step[]的gTemp[]
            for (int i = 0; i<parametersNumber; ++i) {
                gTemp[i] = 0;
                for (int j = 0; j<dataNumber; ++j) {
                    gTemp[i] += (dataJacobian.at(j))[i] * residual.at(j);
                }
            }
        }

        //计算矩阵hessianLm=hessian+(lamda*identity),identity为parametersNumber阶的单位矩阵
        for (int i = 0; i<parametersNumber; ++i) {
            for (int j = 0; j<parametersNumber; ++j) {
                hessianLm[parametersNumber*i + j] = hessian[parametersNumber*i + j];
            }
            hessianLm[parametersNumber*i + i] += lamda;
        }
        //矩阵hessianLm求逆
        if (inverse(hessianLm, parametersNumber)) {
            return -1;
        }

        //求step=inv(hessian)*(dataJacobian*residual)  checked
        double step[parametersNumber] = { 0 };
        for (int i = 0; i<parametersNumber; ++i) {
            for (int j = 0; j<parametersNumber; ++j) {
                step[i] += hessianLm[parametersNumber*i + j] * gTemp[j];
            }
        }
        //更新parametersLm
        for (int i = 0; i<parametersNumber; ++i) {
            parametersLm[i] = parametersEst[i] + step[i];
        }

        //计算新的可能估计值对应的y和计算残差e  checked
        eLm = 0;
        for (int i = 0; i<dataNumber; ++i) {
            //------------------------修改这里--------------------------------------------------
            temp = data[4].at(i) - PrivateCombFy(data[5].at(i), data[2].at(i), data[0].at(i),
                                                 data[1].at(i), parametersLm);
            eLm += temp*temp;
            //---------------------------------------------------------------------------------
        }

        //达到eOut的精度退出
        if (eOut >= ((eLm>e) ? (eLm - e) : (e - eLm))) {
            if (eLm<e) {
                for (int i = 0; i<parametersNumber; ++i) {
                    parametersEst[i] = parametersLm[i];
                }
            }
            break;
        }
        //根据残差，更新参数
        if (eLm<e) {
            //printf("%f, m=%d\n", eLm, m);
            lamda /= v;
            e = eLm;
            for (int i = 0; i<parametersNumber; ++i) {
                parametersEst[i] = parametersLm[i];
            }
            iterOn = 1;
        } else {
            lamda *= v;
            iterOn = 0;
        }
    }

    //退出
    for (int i = 0; i<parametersNumber; ++i) {		//将结果复制到目标数组
        parametersPrivateCombFy[i] = parametersEst[i];
    }
    if (!dataJacobian.empty()) {
        for (int i = 0; i<dataNumber; ++i) {
            delete[](dataJacobian.at(i));
        }
    }
    timep = clock() - timep;
    //printf("%f, time=%ldms\n", e - eLm, timep);
    return timep;
}

int TireRecognition::computePrivateCombMz(const int iterNumber, const double eOut, double lamda, const double v) {
    clock_t timep = clock();

    const int parametersNumber = 4;	//修改这里。函数参数的个数。PrivateCombFy方程有14个系数
    double parametersEst[parametersNumber];		//迭代中进行计算的参数
    double parametersLm[parametersNumber];		//迭代中进行计算的参数,基本上LM是EST的下一个

    //对估计参数赋初值，parametersEst为最后求得的参数数组
    InitialPrivateCombMz(parametersEst);

    int dataNumber = data[0].size();	//数据个数

    //计算雅可比矩阵和残差，将雅可比矩阵改为一个数组内包含所有变量
    std::vector<double*> dataJacobian;	//方程的雅可比矩阵
    std::vector<double> residual;		//每次实际data[0]和理论data[0]的误差
    double temp;	//临时浮点数
    double e = 0, eLm;		//残差之和，Lm是新的
    bool iterOn = 1;	//是否进行迭代
    double hessian[parametersNumber*parametersNumber];	//海塞矩阵，用一位数组表示
    double hessianLm[parametersNumber*parametersNumber];
    double gTemp[parametersNumber] = { 0 };		//用于计算步长step[]

    //分配内存，加快速度。总是分配内存影响性能
    for (int i = 0; i<dataNumber; ++i) {
        double* parametersJacobian = new double[parametersNumber];	//新建参数数组，记得删除
        double residual_temp;
        dataJacobian.push_back(parametersJacobian);
        residual.push_back(residual_temp);
    }

    //计算循环开始
    for (int m = 0; m<iterNumber; ++m) {
        if (iterOn) {
            //计算雅可比矩阵 和残差   checked
            for (int i = 0; i<dataNumber; ++i) {
                //------------------------修改这里--------------------------------------------------------
                //                double alpha=data[0].at(i);
                double r = data[1].at(i);
                //                double k=data[2].at(i);
                //                double Fx=data[3].at(i);
                //                double Fy=data[4].at(i);
                //                double Fz=data[5].at(i);

                //计算每个点的残差
                residual.at(i) = data[6].at(i) - PrivateCombMz(data[5].at(i), data[2].at(i),
                                 data[0].at(i), r, parametersEst);

                //计算雅可比矩阵，矩阵为parametersNumber行，dataNumber列
                double* parametersJacobian = dataJacobian.at(i);
                parametersJacobian[0] = PrivateCombMz_00();
                parametersJacobian[1] = PrivateCombMz_01();
                parametersJacobian[2] = PrivateCombMz_02(r);
                parametersJacobian[3] = PrivateCombMz_03(r);
                //-------------------------------------------------------------------------------------------
            }
            //如果是第一次计算就计算残差和
            if (m == 0) {
                for (int i = 0; i<dataNumber; ++i) {
                    e += residual.at(i)*residual.at(i);
                }
                //printf("%f, m=%d\n", e, m);
            }
            //计算海塞矩阵 checked
            for (int i = 0; i<parametersNumber; ++i) {
                for (int j = 0; j<parametersNumber; ++j) {
                    temp = 0;
                    for (int k = 0; k<dataNumber; ++k) {
                        temp += (dataJacobian.at(k))[i] * (dataJacobian.at(k))[j];
                    }
                    hessian[parametersNumber*i + j] = temp;
                }
            }
            //计算用于计算步长step[]的gTemp[]
            for (int i = 0; i<parametersNumber; ++i) {
                gTemp[i] = 0;
                for (int j = 0; j<dataNumber; ++j) {
                    gTemp[i] += (dataJacobian.at(j))[i] * residual.at(j);
                }
            }
        }

        //计算矩阵hessianLm=hessian+(lamda*identity),identity为parametersNumber阶的单位矩阵
        for (int i = 0; i<parametersNumber; ++i) {
            for (int j = 0; j<parametersNumber; ++j) {
                hessianLm[parametersNumber*i + j] = hessian[parametersNumber*i + j];
            }
            hessianLm[parametersNumber*i + i] += lamda;
        }
        //矩阵hessianLm求逆
        if (inverse(hessianLm, parametersNumber)) {
            return -1;
        }

        //求step=inv(hessian)*(dataJacobian*residual)  checked
        double step[parametersNumber] = { 0 };
        for (int i = 0; i<parametersNumber; ++i) {
            for (int j = 0; j<parametersNumber; ++j) {
                step[i] += hessianLm[parametersNumber*i + j] * gTemp[j];
            }
        }
        //更新parametersLm
        for (int i = 0; i<parametersNumber; ++i) {
            parametersLm[i] = parametersEst[i] + step[i];
        }

        //计算新的可能估计值对应的y和计算残差e  checked
        eLm = 0;
        for (int i = 0; i<dataNumber; ++i) {
            //------------------------修改这里--------------------------------------------------
            temp = data[6].at(i) - PrivateCombMz(data[5].at(i), data[2].at(i), data[0].at(i),
                                                 data[1].at(i), parametersEst);
            eLm += temp*temp;
            //---------------------------------------------------------------------------------
        }

        //达到eOut的精度退出
        if (eOut >= ((eLm>e) ? (eLm - e) : (e - eLm))) {
            if (eLm<e) {
                for (int i = 0; i<parametersNumber; ++i) {
                    parametersEst[i] = parametersLm[i];
                }
            }
            break;
        }
        //根据残差，更新参数
        if (eLm<e) {
            //printf("%f, m=%d\n", eLm, m);
            lamda /= v;
            e = eLm;
            for (int i = 0; i<parametersNumber; ++i) {
                parametersEst[i] = parametersLm[i];
            }
            iterOn = 1;
        } else {
            lamda *= v;
            iterOn = 0;
        }
    }

    //退出
    for (int i = 0; i<parametersNumber; ++i) {		//将结果复制到目标数组
        parametersPrivateCombMz[i] = parametersEst[i];
    }
    if (!dataJacobian.empty()) {
        for (int i = 0; i<dataNumber; ++i) {
            delete[](dataJacobian.at(i));
        }
    }
    timep = clock() - timep;
    //printf("%f, time=%ldms\n", e - eLm, timep);
    return timep;
}

int TireRecognition::inverse(double mat[], const int dimension) {	//checked
    //	double fDet =1.0f;	//用于计算矩阵的行列式，现在用不着
    //	int f =1;
    int is[dimension] = {0}, js[dimension] = {0};
//    int* is = new int[dimension];     //C++不支持不定数组，只有gcc编译器支持
//    int* js = new int[dimension];
//    memset(is, 0, dimension*sizeof(int));
//    memset(js, 0, dimension*sizeof(int));
    for (int k = 0; k<dimension; ++k) {
        // 第一步，全选主元
        double fMax = 0.0f;
        for (int i = k; i<dimension; ++i) {
            for (int j = k; j<dimension; ++j) {
                const double temp = (mat[dimension*i + j] >= 0) ? mat[dimension*i + j] : (-mat[dimension*i + j]);
                if (temp>fMax) {
                    fMax = temp;
                    is[k] = i;
                    js[k] = j;
                }
            }
        }
        if (0.000001f >((fMax >= 0) ? fMax : (-fMax))) {
            return -1;
        }
        //交换mat中的数
        if (is[k] != k) {
            //			f = -f;
            for (int i = 0; i<dimension; ++i) {
                const double temp = mat[dimension*k + i];
                mat[dimension*k + i] = mat[dimension*is[k] + i];
                mat[dimension*is[k] + i] = temp;
            }
        }
        if (js[k] != k) {
            //			f = -f;
            for (int i = 0; i<dimension; ++i) {
                const double temp = mat[dimension*i + k];
                mat[dimension*i + k] = mat[dimension*i + js[k]];
                mat[dimension*i + js[k]] = temp;
            }
        }
        // 计算行列值
        //		fDet *= mat[dimension*k+k];
        // 计算逆矩阵
        // 第二步
        mat[dimension*k + k] = 1.0f / mat[dimension*k + k];
        // 第三步
        for (int i = 0; i<dimension; ++i) {
            if (i != k) {
                mat[dimension*k + i] *= mat[dimension*k + k];
            }
        }
        // 第四步
        for (int i = 0; i<dimension; ++i) {
            if (i != k) {
                for (int j = 0; j<dimension; ++j) {
                    if (j != k) {
                        mat[dimension*i + j] = mat[dimension*i + j] - mat[dimension*i + k] * mat[dimension*k + j];
                    }
                }
            }
        }
        // 第五步
        for (int i = 0; i<dimension; ++i) {
            if (i != k)
                mat[dimension*i + k] *= -mat[dimension*k + k];
        }
    }
    //交换数据
    for (int k = dimension - 1; k >= 0; --k) {
        if (js[k] != k) {
            for (int i = 0; i<dimension; ++i) {
                const double temp = mat[dimension*k + i];
                mat[dimension*k + i] = mat[dimension*js[k] + i];
                mat[dimension*js[k] + i] = temp;
            }
        }
        if (is[k] != k) {
            for (int i = 0; i<dimension; ++i) {
                const double temp = mat[dimension*i + k];
                mat[dimension*i + k] = mat[dimension*i + is[k]];
                mat[dimension*i + is[k]] = temp;
            }
        }
    }

//    delete[] is;
//    delete[] js;
    //	return fDet * f;		//返回行行列式的值
    return 0;
}

/*将所有的test函数注释起来
void LM_Method::test(){
    FileOpen("E:\\matlab to C++\\data.txt");
    ComputePrivateFy(iterNumber, eOut, lamdaInit, v);
    ComputePrivateMz(iterNumber, eOut, lamdaInit, v);
    for (int i = 0; i<25; ++i){
        printf("parameters[%d]=%f\n", i, parametersPrivateMz[i]);
    }
    FILE* file =0;
    fopen_s(&file,"E:\\matlab to C++\\xishu_Fy.txt", "w");
    for (int i = 0; i<18; ++i){
        fprintf(file, "%.12lf\n", parametersPrivateFy[i]);
    }
    fopen_s(&file,"E:\\matlab to C++\\xishu_Mz.txt", "w");
    for (int i = 0; i<25; ++i){
        fprintf(file, "%.12lf\n", parametersPrivateMz[i]);
    }
    fclose(file);
}

void LM_Method::test2(){
    double data_1[9] = { 0.25, 0.5, 1, 1.5, 2, 3, 4, 6, 8 }
    double obs_1[9] = { 19.21, 18.15, 15.36, 14.10, 12.89, 9.32, 7.45, 5.24, 3.01 }
    //	double data_1[12]={0.02, 0.02, 0.06, 0.06, 0.11, 0.11, 0.22, 0.22, 0.56, 0.56, 1.10, 1.10}
    //	double obs_1[12]={76, 47, 97, 107, 123, 139, 159, 152, 191, 201, 207, 200}
    for (int i = 0; i<9; ++i){
        data[0].push_back(obs_1[i]);
        data[1].push_back(data_1[i]);
    }
    Compute(lamdaInit);
    //	printf("a=%.12f, b=%.12f\n", parametersEst[0], parametersEst[1]);
}

#define Fz 200
#define k 500
#define alpha -300
#define r 100
void LM_Method::test3(){
    InitialPrivateFy(parametersPrivateFy);
    InitialPrivateCombFy(parametersPrivateCombFy);
    PrivateCombFy(Fz,k,alpha,r,parametersPrivateCombFy);
    double temp=PrivateCombFy(Fz,k,alpha,r,parametersPrivateCombFy);
    printf("%.14f\n", temp);
}
#undef Fz
#undef alpha
#undef k
#undef r

void LM_Method::test4(){
    FileOpen("E:\\matlab to C++\\data.txt");
    InitialPrivateFy(parametersPrivateFy);
    FILE* file;
    fopen_s(&file,"E:\\matlab to C++\\xishu.txt", "w");
    for (int i = 0; i<18; ++i){
        fprintf(file, "%lf\n", parametersPrivateMz[i]);
    }
    fclose(file);
}
*/

int TireRecognition::parameterSave(const char* fname) {
    FILE* file =0;
    fopen_s(&file, fname, "w");
    if (!file) {
        return -1;
    }
    //获得当前时间的字符串
    char timeStr[21];
    time_t rawtime=time(0);
    tm time;
    localtime_s(&time,&rawtime);
    strftime(timeStr, 21,"%Y.%m.%d  %H:%M:%S", &time);

    fprintf(file, "Created by 轮胎魔术公式识别程序\n%s\n", timeStr);
    if (isUsedFunction(LM_PRIVATEFX)) {
        char para[15][5] = {
            "Pcx1", "Pdx1", "Pdx2", "Pdx3", "Pex1", "Pex2", "Pex3", "Pex4",
            "Pkx1", "Pkx2", "Pkx3", "Phx1", "Phx2", "Pvx1", "Pvx2"
        };
        fprintf(file, "\nPrivateFx的参数：\n");
        for (int i = 0; i<15; ++i) {
            fprintf(file, "%s\t%.12lg\n", para[i], parametersPrivateFx[i]);
        }
    }
    if (isUsedFunction(LM_PRIVATEFY)) {
        char para[18][5] = {
            "Pcy1", "Pdy1", "Pdy2", "Pdy3", "Pey1", "Pey2", "Pey3", "Pey4", "Pky1",
            "Pky2", "Pky3", "Phy1", "Phy2", "Phy3", "Pvy1", "Pvy2", "Pvy3", "Pvy4"
        };
        fprintf(file, "\nPrivateFy的参数：\n");
        for (int i = 0; i < 18; ++i) {
            fprintf(file, "%s\t%.12lg\n", para[i], parametersPrivateFy[i]);
        }
    }
    if (isUsedFunction(LM_PRIVATEMZ)) {
        char para[25][6] = {
            "qbz1", "qbz2", "qbz3", "qbz4", "qbz5", "qbz9", "qbz10", "qcz1", "qdz1",
            "qdz2", "qdz3", "qdz4", "qdz6", "qdz7", "qdz8", "qdz9", "qez1", "qez2",
            "qez3", "qez4", "qez5", "qhz1", "qhz2", "qhz3", "qhz4"
        };
        fprintf(file, "\nPrivateMz的参数：\n");
        for (int i = 0; i < 25; ++i) {
            fprintf(file, "%s\t%.12lf\n", para[i], parametersPrivateMz[i]);
        }
    }
    if (isUsedFunction(LM_PRIVATECOMBFX)) {
        char para[6][5] = {
            "Rbx1", "Rbx2", "Rcx1", "Rex1", "Rex2", "Rhx1"
        };
        fprintf(file, "\nPrivateCombFx的参数：\n");
        for (int i = 0; i < 6; ++i) {
            fprintf(file, "%s\t%.12lg\n", para[i], parametersPrivateCombFx[i]);
        }
    }
    if (isUsedFunction(LM_PRIVATECOMBFY)) {
        char para[14][5] = {
            "Rby1", "Rby2", "Rby3", "Rcy1", "Rey1", "Rey2", "Rhy1", "Rhy2", "Rvy1", "Rvy2",
            "Rvy3", "Rvy4", "Rvy5", "Rvy6"
        };
        fprintf(file, "\nPrivateCombFy的参数：\n");
        for (int i = 0; i < 14; ++i) {
            fprintf(file, "%s\t%.12lg\n", para[i], parametersPrivateCombFy[i]);
        }
    }
    if (isUsedFunction(LM_PRIVATECOMBMZ)) {
        char para[4][5] = {
            "Ssz1", "Ssz2", "Ssz3", "Ssz4"
        };
        fprintf(file, "\nPrivateCombMz的参数：\n");
        for (int i = 0; i < 4; ++i) {
            fprintf(file, "%s\t%.12lg\n", para[i], parametersPrivateCombMz[i]);
        }
    }
    fclose(file);
    return 0;
}

int TireRecognition::compute( const int iterNumber, const double eOut, const double lamda, const double v,
                              const double Fzo, const double Ro ) {
    //检查是否读入了数据
    for(int i=0; i<7; ++i) {
        if( data[i].size() < 100 ) return -10;   //返回错误：有数据量小于100
    }
    //先做参数检查
    if(iterNumber < 10) return -1;      //返回错误：迭代次数太小
    if(eOut < 0) return -2;             //返回错误：误差小于0
    if(lamda <= 0) return -3;           //返回错误：lamda <= 0
    if(v <= 1) return -4;               //返回错误：lamda变化系数v <= 1
    if(Fzo <= 0) return -5;             //返回错误：Fzo <= 0
    if(Ro <= 0) return -6;              //返回错误：Ro <= 0

    MagicFormula::inputFzoRo(Fzo, Ro);
    int info = 0;
    if (usedFunction&LM_PRIVATEFX) {
        info += computePrivateFx(iterNumber, eOut, lamda, v);
    }
    if (usedFunction&LM_PRIVATEFY) {
        info += computePrivateFy(iterNumber, eOut, lamda, v);
    }
    if (usedFunction&LM_PRIVATEMZ) {
        info += computePrivateMz(iterNumber, eOut, lamda, v);
    }
    if (usedFunction&LM_PRIVATECOMBFX) {
        info += computePrivateCombFx(iterNumber, eOut, lamda, v);
    }
    if (usedFunction&LM_PRIVATECOMBFY) {
        info += computePrivateCombFy(iterNumber, eOut, lamda, v);
    }
    if (usedFunction&LM_PRIVATECOMBMZ) {
        info += computePrivateCombMz(iterNumber, eOut, lamda, v);
    }
    return info;
}

void TireRecognition::setUsedFunction(unsigned char function) {
    usedFunction |= function;
}

bool TireRecognition::isUsedFunction(unsigned char function) {
    return (usedFunction&function) ? true : false;
}
